'''
Description:  play motion by http
Version: V1.0
Author: zw_1520@163.com
Date: 2025-04-22 10:22:22
LastEditors: zw_1520@163.com
LastEditTime: 2025-05-15 10:04:21
All rights reserved.
'''

import base64
import json
import requests
import time
from datetime import datetime
import motion_control_service


def set_status(ip, status):
    """Sets interaction status"""
    # InteractionStatus definition
    # InteractionStatus_Ready = 0  # Ready
    # InteractionStatus_Talk  = 1  # Talk
    # InteractionStatus_Task  = 2  # Task

    # Constructs data
    data = {
        "status": status
    }
    # Sends post request
    session = requests.Session()
    response = session.post(
        url='http://'+ip+':59201/rpc/aimdk.protocol.InteractionStatusService/SetStatus',
        headers={'content-type': 'application/json'},
        data=json.dumps(data, ensure_ascii=False)
    )


def get_status(ip):
    """Gets interaction status"""
    # Sends post request.
    session = requests.Session()
    response = session.post(
        url='http://'+ip+':59201/rpc/aimdk.protocol.InteractionStatusService/GetStatus',
        headers={'content-type': 'application/json'},
        data=json.dumps({}))
    # Prints response
    # print(f"Response status: {response.status_code}")
    # print(f"Response header: {response.headers}")
    print(f"Response:", json.loads(response.text)["status"])
    return json.loads(response.text)["status"]


def get_task_status(ip):
    """Gets task status"""
    # Sends post request
    session = requests.Session()
    response = session.post(
        url='http://'+ip+':59201/rpc/aimdk.protocol.InteractionTaskService/GetInteractionTaskStatus',
        headers={'content-type': 'application/json'},
        data=json.dumps({}))
    # Prints log
    # print(f"[GetStatus 响应]")
    # print(f"Status code: {response.status_code}")
    # print(f"Response headers: {response.headers}")
    print(f"Response:", json.loads(response.text)["status"])
    # return json.loads(response.text)["status"]


def encoded_text(text):
    text = text
    encoded_bytes = base64.b64encode(text.encode('utf-8'))
    encoded_text = encoded_bytes.decode('utf-8')

    return encoded_text


def post_data(text, motion, face):
    """Posts data """
    data = {
        "task": {
            "task_name": "interaction_task",
            "interaction_list": [
                {
                    "text": encoded_text(text),
                    "motion_name": motion,  # Motion id
                    "face_expression": face  # Faces id
                }
            ],
            "task_type": 0
        }
    }
    return data


def motion_data(motion_id, duration_ms):
    """ Constructs motion data"""
    data = {
        "motion_id": "{motion_id}",
        "duration_ms": duration_ms,
        "cmd_end": True,
        "cmd_pause": False,
        "cmd_reset": False
    }
    return data


def play_motion_by_http(motion_id, duration_ms):
    """Plays motion"""

    # Sets robot status
    motion_service = motion_control_service.MotionControlService()
    motion_service.ensure_action('RL_LOCOMOTION_ARM_EXT_JOINT_SERVO')

    # InteractionStatus definition
    # InteractionStatus_Ready = 0  # Ready
    # InteractionStatus_Talk  = 1  # Talk
    # InteractionStatus_Task  = 2  # Task

    # Constructs data
    data = motion_data(
        motion_id, duration_ms)
    print(f"data: {data}")
    # Sends post request
    session = requests.Session()
    orin_ip = '192.168.100.100'
    # http://$IP_ADDRESS/rpc/aimdk.protocol.MotionCommandService/SendMotionCommand
    # response = session.post(
    #     url=ip+'/rpc/aimdk.protocol.MotionCommandService/SendMotionCommand',
    #     headers={'content-type': 'application/json'},
    #     data=json.dumps(data, ensure_ascii=False)
    # )
    response = requests.post(
        url='http://'+orin_ip +
            ':56444/rpc/aimdk.protocol.MotionCommandService/SendMotionCommand',
        headers={'content-type': 'application/json'},
        data=json.dumps(data,
                        ensure_ascii=False)  # Closes ascii for chinese
    )


def arm_control():
    url = (
        "http://192.168.100.100:56322/channel/%2Fmotion%2Fcontrol%2Fjoint_arm_control/pb%3Aaimdk.protocol.McArmControlChannel"
    )

    # 发送的位置数据
    flag = 2
    vel = [0.0] * 14
    acc = [0.0] * 14
    eff = [0.0] * 14
    position = [0, 1.2, 0, -0.5, 1.5, 0, 0, 0, -1.2, 0, 0.5, 1.2, 0, 0]
    posIni = [0, 1.2, 0, -0.5, 1.5, 0, 0, 0, -1.2, 0, 0.5, 1.5, 0, 0]
    pos = 0
    increasing = True
    step = 0.01
    # # 发送的姿态数据

    try:
        while True:
            # for pos in positions:

            header = create_header()

            position[3] = posIni[3] + pos
            position[10] = posIni[10] - pos
            mcmove_arm_data = create_mcarm_control_channel(
                header, flag, position, vel, acc, eff)

            if increasing:
                pos += step
                if pos >= 0.2:
                    pos = 0.2
                    increasing = False
            else:
                pos -= step
                if pos <= -0.2:
                    pos = -0.2
                    increasing = True

            time.sleep(0.1)
            response = send_pose_data(url, mcmove_arm_data)
            print(f"Sent data: {json.dumps(mcmove_arm_data, indent=2)}")
            print(f"Response: {response.status_code} - {response.text}\n")

    except KeyboardInterrupt:
        print("Stopped by user")


if __name__ == "__main__":
    # Sets robot status
    motion_service = motion_control_service.MotionControlService()
    motion_service.ensure_action('STAND_ARM_EXT_JOINT_ONLINE_TRAJ')
    # arm_control()
